'''Date : O5th october 2012
		 Authors : Isaline Laurent (ens12ilt) - Gizem Berkdemir (ens12gbr)

		 This file contains all the tools not directly linked to the environment 
		 '''
from math import sqrt, atan2, cos, sin

def getDistance(point) :
		'''Return the distance between the given point and the origin'''
		return sqrt(point[0]**2 + point[1]**2)

def getConvertPath(jpath, position, limit) :
		''' Convert the first limit point from a path, converted into the coordinate system given by the position 
          Input : jpath, a loaded json file
                  postion, the current position and orientation of the robot
                  limit, the limit number of points taken from the path
          Output : the path, converted in order the robot to be the origin and according to its orientation
    '''
		p = position['Pose']['Position']
		phi = getPhi(position)
		rpath = min(len(jpath), limit) * [0]
		for i in range(min(len(jpath), limit)):
				rpath[i] = 2 * [0]
				posi = jpath[i]['Pose']['Position']
				x = posi['X'] - p['X'] 
				y = posi['Y'] - p['Y']
				rpath[i][0] =		 x		* cos(phi) + y * sin(phi)
				rpath[i][1] = (-x) * sin(phi) + y * cos(phi)

		return rpath 


def getPhi(position) :
		''' Returns the yaw from a quaternion
          Input : position, the current position and orientation of the robot
          Output : the angle called yaw from the quaternion
    '''
		q = position['Pose']['Orientation']
		return atan2(2*(q['W']*q['Z'] + q['X']*q['Y']), 1 - 2*(q['Y']**2 + q['Z']**2))


def getRadius(point, distance) :		
		''' Returns a radius from a point and its distance from the origin
          Input : point, the carrot point coordinates
                  distance, the distance between the robot and this point
          Output : the radius of a circle including the robot and the carrot point
    '''
		return 1.0 / (2 * (point[1] / distance**2))

def getSleepTime(angle, linear, radius, params, c) :
		''' Returns the angular and linear speed, and the time to wait, in order to reach a point
				 Input : angle, the angle between the point and the origin
                 linear, the lenght of the arc subtended by this angle, and between the point and the origin
                 radius, the radius of this arc
                 params, the speed parameters of the robot
                 c, a factor regarding the linear speed
         Output : the angular speed, the linear speed, and the time to wait for reaching the carrot point
    '''
		w = params['MaxLinearSpeed'] * c / radius
		l = params['MaxAngularSpeed'] * radius / c

		if angle <= params['MaxAngularSpeed'] and linear <= params['MaxLinearSpeed'] :
				if w <= params['MaxAngularSpeed'] :
						return linear / params['MaxLinearSpeed'], w, c*params['MaxLinearSpeed'] 
				elif l <= params['MaxLinearSpeed'] :		
						return angle / params['MaxAngularSpeed'], params['MaxAngularSpeed'], l
				else :
						return 1, angle, linear

		elif angle > params['MaxAngularSpeed'] and l <= params['MaxLinearSpeed'] :
				return angle / params['MaxAngularSpeed'], params['MaxAngularSpeed'], l
		elif linear > params['MaxLinearSpeed'] and w <= params['MaxAngularSpeed'] :
				return linear / params['MaxLinearSpeed'], w, c*params['MaxLinearSpeed'] 
		else :
				return 0,0,0
